#include "Hpid.h"

PID_t *mthis ; 

PID_t* GetPid(PID_t *pid) {
    mthis = pid ;
    return mthis;
}

int HPID_Run(int current) {
    int ans  = 0;
    mthis->current = current ; 
    mthis->e0 = mthis->e1;
    mthis->e1 = mthis->e2;
    mthis->e2 = mthis->target - mthis->current ; 
    ans  = mthis -> Kp * (mthis->e2 - mthis->e1) ;  //P
    ans += mthis->Ki * mthis->e2 ;            //I 
    ans += mthis->Kd * (mthis->e2 -2*mthis->e1 + mthis->e0) ; //D 
    ans = ans / mthis->A ;
    return ans ;
}

void HPID_Reset() {
    mthis->e2 = 0;
    mthis->e1 = 0;
    mthis->e0 = 0;
}

void HPID_Init(PID_t *pid ){
    pid->A = 1;
    pid->output = 0;
    pid->current = 0;
    pid->target = 0;
    pid->Kp=0;
    pid->Ki= 0 ;
    pid->Kd = 0;
    pid->Run = HPID_Run;
    pid->Reset = HPID_Reset ;  
}